#include "motion/motion_cmd/rtdev_setting_cmd.h"
#include "motion/motion_cmd/motion_cmd.h"
#include "motion/motion_cmd/moving_cmd.h"
#include "safety/safety_cmd.h"
#include "motion_al.h"
#include "log/zuclog.h"
#include "rob_common/robtool.h"
#include "rob_common/robot_payload.h"
#include "rob_common/usrframe.h"

#define TASK_INFO(fmt, ...) zuclog_info("TASK", fmt, ##__VA_ARGS__)
#define TASK_ERR(fmt, ...) zuclog_error("TASK", fmt, ##__VA_ARGS__)
#define TASK_DEBUG(fmt, ...) zuclog_debug("TASK", "[%06d] " fmt, __LINE__, ##__VA_ARGS__)

extern void write_robot_config_to_file();

void save_all_config()
{
    write_robot_config_to_file();
    axisgroup::RobotPayloadMgr::save();
    axisgroup::RobotToolMgr::save();
    axisgroup::UsrFrameMgr::save();
}

/*************************************************************
 * 上电上使能 相关指令
**************************************************************/
int zucRobotPowerOff(int robid)
{
    save_all_config();
    int start_index = robid < 0 ? 0 : robid;
    for (int i = start_index;; i++)
    {
        //检查指定的机器人是否存在
        auto rob = mot::al::MotionProxy::instance().robot(i);
        if (rob.get() == nullptr)
        {
            break;
        }
        //通知安全板断电
        int err = mot::realtime_cmd_call(std::make_shared<CmdPowerOffRobot>(i));
        if (err)
        {
            return err;
        }

        if (robid >= 0)
        {
            break;
        }
    }
    return 0;
}

int boot_robot(int robid)
{
    // 加载配置文件
    int err = mot::realtime_cmd_call(std::make_shared<CmdRobotBoot>(robid));
    if (err)
    {
        return err;
    }
    err = mot::realtime_cmd_call(std::make_shared<CmdRobotAxisBoot>(robid));
    if (err)
    {
        return err;
    }

    return 0;
}

int zucRobotPowerOn(int robid)
{
    save_all_config();
    int start_index = robid < 0 ? 0 : robid;
    for (int i = start_index;; i++)
    {
        //检查指定的机器人是否存在
        auto rob = mot::al::MotionProxy::instance().robot(i);
        if (rob.get() == nullptr)
        {
            break;
        }

        //通知机器通电
        int err = mot::realtime_cmd_call(std::make_shared<CmdRobotPowerOn>(i, 0, 0, 0));
        if (err)
        {
            return err;
        }
        err = boot_robot(i);
        if (err)
        {
            return err;
        }

        if (robid >= 0)
        {
            break;
        }
    }
    return 0;
}

int zucTrajEnable(int robid)
{
    save_all_config();
    int start_index = robid < 0 ? 0 : robid;
    for (int i = start_index;; i++)
    {
        auto cmd = std::make_shared<CmdEnable>(i);
        int err = mot::realtime_cmd_call(cmd);
        if (err)
        {
            return err;
        }
        if (robid >= 0)
        {
            break;
        }
    }
    return 0;
}

int zucTrajDisable(int robid)
{
    save_all_config();
    int start_index = robid < 0 ? 0 : robid;
    for (int i = start_index;; i++)
    {
        auto rob = mot::al::MotionProxy::instance().robot(i);
        if (rob.get() == nullptr)
        {
            break;
        }
        //检查指定的机器人是否存在
        auto cmd = std::make_shared<CmdDisable>(i);
        int err = mot::realtime_cmd_call(cmd);
        if (err)
        {
            return err;
        }
        if (robid >= 0)
        {
            break;
        }
    }
    return 0;
}

/*************************************************************
 * JOG 相关指令
**************************************************************/
int zucJogStop(int robid)
{
    auto cmd = std::make_shared<CmdJogStop>(robid);
    return mot::realtime_cmd_call(cmd);
}

int zucJogAbs(int robid, int nr, double pos, double vel, int jjogmode)
{
    auto cmd = std::make_shared<CmdJogAbs>(robid, jjogmode, nr, vel, pos);
    return mot::realtime_cmd_call(cmd);
}

// jjogmode 控制关节还是笛卡尔jog
int zucJogCont(int robid, int nr, double vel, int jjogmode)
{
    auto cmd = std::make_shared<CmdJogCont>(robid, jjogmode, nr, vel);
    return mot::realtime_cmd_call(cmd);
}

int zucJogIncr(int robid, int nr, double incr, double vel, int jjogmode)
{
    auto cmd = std::make_shared<CmdJogIncr>(robid, jjogmode, nr, vel, incr);
    return mot::realtime_cmd_call(cmd);
}

int zucJogMultiJoint(int robid, double vel, RobJointVal pos)
{
    auto cmd = std::make_shared<CmdJogMultiJoint>(robid, vel, pos);
    return mot::realtime_cmd_call(cmd);
}

int zucSetDragEnable(int robot_id, int enable)
{
    auto cmd = std::make_shared<CmdSetDragEnable>(robot_id, enable);
    return mot::realtime_cmd_call(cmd);
}

int zucSetServoEnable(int robot_id, int enable)
{
    auto cmd = std::make_shared<CmdServojop>(robot_id, enable);
    return mot::realtime_cmd_call(cmd);
}

// 负载设置相关指令
int zucSetRobotPayload(int robot_id, int id)
{
    auto cmd = std::make_shared<CmdSetRobotPayload>(robot_id, id);
    return mot::realtime_cmd_call(cmd);
}

int zucTrajSetPayload(int id, Payload payload)
{
    if (id < 0 || id >= (int)axisgroup::RobotPayloadMgr::MAX_ROBOT_PAYLOAD_NUM)
    {
        TASK_ERR("payload id %d out of range", id);
        return -1;
    }
    axisgroup::RobotPayload::Payload load = {payload.m, payload.p[0], payload.p[1], payload.p[2]};
    axisgroup::RobotPayloadMgr::get_robot_payload(id).config_payload(load);

    // 通知axisgroup负载已经改变
    for (int i = 0;; i++)
    {
        //检查指定的机器人是否存在
        auto rob = mot::al::MotionProxy::instance().robot(i);
        if (rob.get() == nullptr)
        {
            break;
        }
        if (rob->get_status().robot_config.work_payload_id == id)
        {
            // 只有机器人使用的负载更新时，才需要更新
            int err = mot::realtime_cmd_call(std::make_shared<CmdSetPayload>(i, id));
            if (err)
            {
                return err;
            }
        }
    }
    return 0;
}

int zucServoHome(int robot_id, int joint)
{
    auto cmd = std::make_shared<CmdServoHome>(robot_id, joint);
    return mot::realtime_cmd_call(cmd);
}

int zucTrajSetRobotToolID(int group_id, int id)
{
    if (id >= (int)axisgroup::RobotToolMgr::MAX_ROBOT_TOOL_NUM)
    {
        return -1;
        TASK_ERR("tool id %d out of range", id);
    }
    auto cmd = std::make_shared<CmdSetRobotToolId>(group_id, id);
    return mot::realtime_cmd_call(cmd);
}

int zucTrajSetRobotToolOffset(ZucPose tool_offset, unsigned char method, int id, const char* name)
{
    if (id < 0 || id >= (int)axisgroup::RobotToolMgr::MAX_ROBOT_TOOL_NUM)
    {
        TASK_ERR("tool id %d out of range", id);
        return -1;
    }
    axisgroup::RobotToolMgr::get_rob_tool(id).set_pose(tool_offset);
    axisgroup::RobotToolMgr::get_rob_tool(id).config_name(name);

    // 通知axisgroup负载已经改变
    for (int i = 0;; i++)
    {
        //检查指定的机器人是否存在
        auto rob = mot::al::MotionProxy::instance().robot(i);
        if (rob.get() == nullptr)
        {
            break;
        }
        if (rob->get_status().robot_config.tool_frame_id == id)
        {
            // 只有机器人使用的负载更新时，才需要更新
            int err = mot::realtime_cmd_call(std::make_shared<CmdSetRobotToolId>(i, id));
            if (err)
            {
                return err;
            }
        }
    }
    return 0;
}

int zucTrajSetRobotUserID(int robot_id, int id)
{
    if (id >= (int)axisgroup::RobotToolMgr::MAX_ROBOT_TOOL_NUM)
    {
        TASK_ERR("frame id %d out of range", id);
        return -1;
    }
    auto cmd = std::make_shared<CmdSetRobotUserId>(robot_id, id);
    return mot::realtime_cmd_call(cmd);
}

int zucTrajSetRobotUserOffset(ZucPose user_offset, unsigned char method, int id, const char* name)
{
    if (id < 0 || id >= (int)axisgroup::UsrFrameMgr::MAX_USR_FRAME_NUM)
    {
        TASK_ERR("tool id %d out of range", id);
        return -1;
    }
    axisgroup::UsrFrameMgr::get_usr_frame(id).set_pose(user_offset);
    axisgroup::UsrFrameMgr::get_usr_frame(id).config_name(name);

    // 通知axisgroup负载已经改变
    for (int i = 0;; i++)
    {
        //检查指定的机器人是否存在
        auto rob = mot::al::MotionProxy::instance().robot(i);
        if (rob.get() == nullptr)
        {
            break;
        }
        if (rob->get_status().robot_config.user_frame_id == id)
        {
            // 只有机器人使用的负载更新时，才需要更新
            int err = mot::realtime_cmd_call(std::make_shared<CmdSetRobotUserId>(i, id));
            if (err)
            {
                return err;
            }
        }
    }
    return 0;
}

int zucSetServoParam(int robot_id, ServoParam param)
{
    auto cmd = std::make_shared<CmdServoSetParam>(robot_id, param);
    return mot::realtime_cmd_call(cmd);
}

int zucClearError(int robot_id)
{
    auto cmd = std::make_shared<CmdClearError>(robot_id);
    return mot::realtime_cmd_call(cmd);
}

int zucMotionAbort(int robot_id)
{
    int start_index = robot_id < 0 ? 0 : robot_id;
    for (int i = start_index;; i++)
    {
        //检查指定的机器人是否存在
        auto rob = mot::al::MotionProxy::instance().robot(i);
        if (rob.get() == nullptr)
        {
            break;
        }
        //通知安全板断电
        int err = mot::realtime_cmd_call(std::make_shared<CmdAbortJointAndTraj>(i));
        if (err)
        {
            return err;
        }

        if (robot_id >= 0)
        {
            break;
        }
    }
    return 0;
}

int zucTrajSetRapidScale(double scale, int robot_id)
{
    int start_index = robot_id < 0 ? 0 : robot_id;
    for (int i = start_index;; i++)
    {
        //检查指定的机器人是否存在
        auto rob = mot::al::MotionProxy::instance().robot(i);
        if (rob.get() == nullptr)
        {
            break;
        }
        //通知安全板断电
        int err = rob->zucTrajSetRapidScale(scale);
        if (err)
        {
            return err;
        }

        if (robot_id >= 0)
        {
            break;
        }
    }
    return 0;
}

int zucTrajPause(int robot_id)
{
    int start_index = robot_id < 0 ? 0 : robot_id;
    for (int i = start_index;; i++)
    {
        //检查指定的机器人是否存在
        auto rob = mot::al::MotionProxy::instance().robot(i);
        if (rob.get() == nullptr)
        {
            break;
        }
        //通知安全板断电
        int err = rob->do_pause();
        if (err)
        {
            return err;
        }

        if (robot_id >= 0)
        {
            break;
        }
    }
    return 0;
}

int zucTrajResume(int robot_id)
{
    int start_index = robot_id < 0 ? 0 : robot_id;
    for (int i = start_index;; i++)
    {
        //检查指定的机器人是否存在
        auto rob = mot::al::MotionProxy::instance().robot(i);
        if (rob.get() == nullptr)
        {
            break;
        }
        //通知安全板断电
        int err = rob->do_resume();
        if (err)
        {
            return err;
        }

        if (robot_id >= 0)
        {
            break;
        }
    }
    return 0;
}

// 代替原来的install angle 接口
int zucSetBaseOffset(int robot_id, const double* base_offset)
{
    // 通知axisgroup负载已经改变
    for (int i = 0;; i++)
    {
        //检查指定的机器人是否存在
        auto rob = mot::al::MotionProxy::instance().robot(i);
        if (rob.get() == nullptr)
        {
            break;
        }
        auto id = rob->get_status().robot_config.base_frame_id;
        ZucPose offset = {};
        offset.tran.x = base_offset[0];
        offset.tran.y = base_offset[1];
        offset.tran.z = base_offset[2];
        offset.a = base_offset[3];
        offset.b = base_offset[4];
        offset.c = base_offset[5];
        axisgroup::UsrFrameMgr::get_usr_frame(id).set_pose(offset);
        // 通知机器人，base offset改变，需要通知dyn库

        auto cmd = std::make_shared<CmdSetBaseOffset>(i, base_offset);
        return mot::realtime_cmd_call(cmd);
    }
    return 0;
}

int zucSetSimRobot(int robot_id, int sim_serial_num)
{
    auto cmd = std::make_shared<CmdSetSimRobot>(robot_id, sim_serial_num);
    return mot::realtime_cmd_call(cmd);
}
